--MainStatusMachine model
--module(..., package.seeall)

-- 在50ms定时器内调用主状态机

require 'Motor'
require 'SCA116T'

TrackCtrl ={
    
    --RunTick   = 0,         -- 启动自动跟踪的时间(秒)

    Status      = "CW_STOP",   --状态机:"STOP,CW,CCW,WIND,HOTGAP,ERROR,ATNIGHT"等
    
    Target      =0,         --目标角度
    Incline     =0,        --当前角度（通常=倾角传感器读数-零点校准）
    Err         =0,         -- Err = incline - target

    CW_Speed    =0,            --正转速度 角度/秒
    CCW_Speed   =0,           --反转速度 角度/秒

    Gap = 0.5,          --跟踪精度°

    --tick =0,            --计时，+正转，-反转

    --A =0,               --校准系数,要放到设备特性表内
    --H =0,
    --D =0
}




---------------------------------------------
--  TrackCtrlProc 自动跟踪控制状态函数列表
---------------------------------------------

--[[ TRACK 基于跟踪精度GAP和误差Err的电机控制策略
--
-- 跟踪控制策略.
--  (1) TrackCtrlProc 有4个状态的状态机
--    1. 正停状态,误差Err< -GAP, 启动正转; Err误差> 3GAP, 启动反转
--    2. 正转状态,Err>GAP,正转停机; 
--    3. 反转状态,Err<-GAP,反转停机
--    4. 反正停机, Err>GAP,启动反转; Err<-3GAP,启动正转
--  (2) 根据目标角度target和 当前角度Incline,决定当前电机的动作(正转,反转200ms或者停),
        因此,最好定时执行,如50ms执行一次  TrackCtrl.Proc(t, i, g)
--  (3) 在console() 可以: 设置TrackCtrl.Run =1,启动自动跟踪控制,
--      设置TrackCtrl.Target跟踪目标角度,也可以读取TrackCtrl的状态等
--]]

local TrackCtrlProc={

    CW_STOP = function()
        if TrackCtrl.Err <= -TrackCtrl.Gap then 
            TrackCtrl.Status = 'CW' 
            log.debug("电机控制"," 正转"," 误差=",TrackCtrl.Err," 精度=",TrackCtrl.Gap)
        end

        if TrackCtrl.Err >=3*TrackCtrl.Gap then 
            TrackCtrl.Status = 'CCW' 
            log.debug("电机控制"," 反转"," 误差=",TrackCtrl.Err," 精度=",TrackCtrl.Gap)
        end
    end,

    CCW_STOP = function()
        if TrackCtrl.Err <= -3*TrackCtrl.Gap then 
            TrackCtrl.Status = 'CW' 
            log.debug("电机控制"," 正转"," 误差=",TrackCtrl.Err," 精度=",TrackCtrl.Gap)
        end

        if TrackCtrl.Err >= TrackCtrl.Gap then 
            TrackCtrl.Status = 'CCW' 
            log.debug("电机控制"," 反转"," 误差=",TrackCtrl.Err," 精度=",TrackCtrl.Gap)
        end
    end,

    CW = function()
        if TrackCtrl.Err >= TrackCtrl.Gap then 
            TrackCtrl.Status = 'CW_STOP' 
            log.debug("电机控制"," 正转停机"," 误差=",TrackCtrl.Err," 精度=",TrackCtrl.Gap)
            Motor.Run = 0
        else
            Motor.Run = 200     -- 正转200ms
        end
    end,

    CCW = function()
        if TrackCtrl.Err <= -TrackCtrl.Gap then 
            TrackCtrl.Status = 'CCW_STOP' 
            log.debug("电机控制"," 反转停机 "," 误差=",TrackCtrl.Err," 精度=",TrackCtrl.Gap)
            Motor.Run = 0
        else
            Motor.Run = -200    -- 反转200ms
        end
    end
}



function TrackTo(t, i, g)
    
    TrackCtrl.Target = t    
    TrackCtrl.Incline = i
    TrackCtrl.Gap = g
    -- 计算误差
    TrackCtrl.Err = TrackCtrl.Incline - TrackCtrl.Target    

    TrackCtrlProc[TrackCtrl.Status]() 
end



-- 50ms线程， TrackCtrl.AutoTrack=1时自动跟踪
--[[
sys.timerLoopStart( function()
    TrackTo(TrackCtrl.Target, SCA116T.x, TrackCtrl.Gap )
end, 50 )
]]